/*========================================================================
			ROBOCON 2008 - PUNE INDIA
			BKIT4 - CNTT DH BACH KHOA TPHCM
			HARDWARE CLUB - AUTO ROBOT2
			File : forward.c
			Description : Forward stragies
=========================================================================*/
#include "define.h"
#include "FreeRTOS_speed.h"
#include "FreeRTOS_encoder.h"
#include "FreeRTOS_pid.h"
#include "FreeRTOS_pid.h"
#include "FreeRTOS_forward.h"




void dc3_speed(int x){
	/*if(x > 255)					
		x = 255;	
	else if(x < -255)
		x = -255;	*/

	if (x >= 0)
	{
		DC3_SPEED = x;
		outdir |= DC_3_FORWARD;//set bit DO55 DIR3
		//DC3_DIR = 1;
	}
	else
	{
		DC3_SPEED = -x; 
		outdir &=  DC_3_BACK; //clear bit DO55 DIR3
		//DC3_DIR = 0;
	}	
	writeIO(OUTDIR,outdir);
}

/*
input:  	L_velocity, R_velocity	: requested input velocities, ticks per 1/20 sec
		velctl_L,velctl_R		: controling velocity
		delay_times			: the loop times for delay ~50x5ms=50x(time of xControllerTask)
*/
void turnleft_process(unsigned char *_function_left,int *velctl_L,int *velctl_R, unsigned char *hold_velocity_flag,unsigned char *delay_times)
{
	switch (*_function_left)
	{
		case TURNLEFT_START:
			reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
			RESET_DISTANCE();
			//quang_duong = 0;
			*velctl_L = -8;
			*velctl_R = 0;
			*hold_velocity_flag= 1;
			*_function_left = TURNLEFT_STATE1;
			break;
		case TURNLEFT_STATE1 : 
			if (-L_ecdr_sbuf>=1930)
			{

				RESET_DISTANCE();
				//quang_duong = 0;
				reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
				*hold_velocity_flag = 1;
				*delay_times = 50;
				*_function_left = TURNLEFT_STATE2;
			}
			break;
		case TURNLEFT_STATE2: 
			if(*delay_times == 0)
			{
				reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
				speed(0,0);
				*_function_left = TURNLEFT_DONE;
			}
			break;
		default :
			*_function_left = TURNLEFT_NO;
			break;
	}
} 
			
void turnright_process(unsigned char *_function_right,int *velctl_L,int *velctl_R, unsigned char *hold_velocity_flag,unsigned char *delay_times)
{
	switch (*_function_right)
	{
		case TURNRIGHT_START:
			reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
			RESET_DISTANCE();
			//quang_duong = 0;
			*velctl_L = 4;
			*velctl_R = -4;
			*hold_velocity_flag= 1;
			*_function_right = TURNRIGHT_STATE1;
			break;
		case TURNRIGHT_STATE1 : 
			if (L_ecdr_sbuf>=3500)
			{
				*velctl_L = 2;
				*velctl_R = -2;
				RESET_DISTANCE();
				//quang_duong = 0;
				reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
				*hold_velocity_flag = 1;
				*_function_right = TURNRIGHT_STATE2;
			}
			break;
		case TURNRIGHT_STATE2: 
			if (L_ecdr_sbuf>=3000)
			{
				*velctl_L = 0;
				*velctl_R = 0;
				RESET_DISTANCE();
				//quang_duong = 0;
				reset_hold_velocity(velctl_L,velctl_R,hold_velocity_flag);
				*hold_velocity_flag = 1;
				speed(0,0);
				*_function_right = TURNRIGHT_DONE;
			}
			break;
		default :
			*_function_right = TURNRIGHT_NO;
		break;
	}
} 

void run_forward_vel_encoder(unsigned char *_function_forward_e,int length,
								int *velctl_L, int *velctl_R, unsigned char *hold_velocity_flag,int vel)
{
	int _quang_duong;
	switch(*_function_forward_e){
		case	FWD_E_START:
			reset_hold_velocity( velctl_L, velctl_R, hold_velocity_flag);
			RESET_DISTANCE();
			*velctl_L = vel;
			*velctl_R = vel;
			*_function_forward_e = FWD_E_STATE0;
			break;
		case 	FWD_E_STATE0:
			*hold_velocity_flag = 1;
			_quang_duong = (L_ecdr_sbuf+R_ecdr_sbuf)/2;
			if(_quang_duong> length)
				*_function_forward_e = FWD_E_STATE1;
			break;
		case	FWD_E_STATE1:
			reset_hold_velocity( velctl_L, velctl_R, hold_velocity_flag);
			RESET_DISTANCE();
			*velctl_L = 0;
			*velctl_R = 0;
			//speed(0,0);
			*_function_forward_e = FWD_E_DONE;
			break;
		case	FWD_E_DONE:
			break;
			
		
		}
	
}




